from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_demo_launch
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    # 生成 MoveIt! 配置的 demo launch
    moveit_config = MoveItConfigsBuilder("zc4model", package_name="zc4_moveit_config").to_moveit_configs()
    moveit_demo_launch = generate_demo_launch(moveit_config)

    # 定义 TimePublisher 节点
    time_publisher_node = Node(
        package='my_custom_msgs', 
        executable='time_publisher',  # 替换为你编译后的节点名
        name='time_publisher',
        output='screen',  
    )

    # 定义通信节点
    topic_example_node = Node(
        package='topic_example', 
        executable='sub_node',
        name='topic_example_node',
        output='log',  
    )
    
    # 定义力传感器节点
    force_three_node = Node(
        package='visual_control', 
        executable='force_three1',
        name='force_three_node',
        output='screen',  
    )
    
    # 定义屏幕节点
    qt_ros2_control_panel = Node(
        package='qt_ros2_gui', 
        executable='third_party',
        name='qt_ros2_control_panel',
        output='screen',  
    )
    
    # 定义眼控节点
    eyecontrol_node = Node(
        package='path_plan', 
        executable='eyecontrol',
        name='eyecontrol_node',
        output='screen',  
    )

    # force_servo_control_node = Node(
    #     package='moveit2_tutorials', 
    #     executable='force_servo',
    #     name='force_servo_control_node',
    #     output='screen',  
    # )

    # 返回启动描述
    return LaunchDescription([
        moveit_demo_launch,
        # time_publisher_node,
        topic_example_node,
        # force_three_node,
        # qt_ros2_control_panel,
        # eyecontrol_node
        # force_servo_control_node
    ])
